package behavior;

import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.util.TextMenu;
import Controller.BarcodeControl;
import Controller.DriveForward;


public class BehaviorController{

	private Arbitrator arbitrator;
	private Behavior[] behaviorList;
	private Behavior leftBumperBehav;
	private Behavior rightBumperBehav;
	private Behavior driveForwardBehav;
	private Behavior barcodeBehav;

	//TODO: change parameters to correct values INCHES
	private final float wheelDiameter = 2;
	private final float trackWidth = 4;

	private TachoPilot pilot = new TachoPilot(wheelDiameter, trackWidth,
			Motor.B, Motor.C);
	private LightSensor lightSensor = new LightSensor(SensorPort.S1);
	private UltrasonicSensor ultrasonicSensor = new UltrasonicSensor(
			SensorPort.S2);
	private TouchSensor touchSensorLeft = new TouchSensor(SensorPort.S3);
	private TouchSensor touchSensorRight = new TouchSensor(SensorPort.S4);

	private String[] menuText = new String[3];
	private TextMenu menu;

	public BehaviorController() throws InterruptedException {		

		driveForwardBehav = new DriveForward(pilot);

		menuText[0] = "Bumper";
		menuText[1] = "BarcodeBehav";
		menuText[2] = "Ultrasonic";

		menu = new TextMenu(menuText);

		int selection = menu.select();
		switch (selection) {

		case 0:
			loadBumper();
			break;
		case 1:
			loadBarcodeBehav();
			break;
		case 2: loadUltrasonic(); break;		
		default:
			System.out.println("No preset selected");
		}		

		arbitrator = new Arbitrator(behaviorList);
		arbitrator.start();

	}

	public void calibrateLightSensor() throws InterruptedException {
		
		Thread.sleep(1000);
		System.out.println("Calibrating lightsensor");
		System.out.println("Set white value");
		Thread.sleep(1000);
		while (!Button.ENTER.isPressed()) {
			lightSensor.calibrateLow();
		}
		Thread.sleep(1000);
		System.out.println("Set black value");
		while (!Button.ENTER.isPressed()) {
			lightSensor.calibrateHigh();
		}
		System.out.println("Lightsensor calibrated");
		System.out.println("Press enter to continue.");
		Button.ENTER.waitForPress();
		
	}

	private void loadBumper() {

		leftBumperBehav = new LeftBumper(touchSensorLeft, pilot);
		rightBumperBehav = new RightBumper(touchSensorRight, pilot);

		behaviorList = new Behavior[3];
		behaviorList[2] = leftBumperBehav;
		behaviorList[1] = rightBumperBehav;
		behaviorList[0] = driveForwardBehav;
	}

	private void loadBarcodeBehav() throws InterruptedException {

		calibrateLightSensor();
		
		pilot.setMoveSpeed(2); //for testing
		
		barcodeBehav = new BarcodeReader(lightSensor, pilot);

		behaviorList = new Behavior[2];
		behaviorList[1] = barcodeBehav;
		behaviorList[0] = driveForwardBehav;
	}
	
	
	
	private void loadUltrasonic()
	{
		ultrasonicSensor.continuous();
		System.out.println("Not implemented");
	}

}
